/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : uwb.cpp
  * Description        : 
  ******************************************************************************
  * Function           :
  *
  ******************************************************************************
  * Author             : Zhiping Fu
  * Creation Date      : 2020年11月22日
  ******************************************************************************
  */
/* USER CODE END Header */

#include "uwb.hpp"
#include "string.h"

UWB uwb(USART6,(char *)"UWB");

void USART6_IRQHandler(void)
{
	if(LL_USART_IsActiveFlag_IDLE(USART6))
	{
		LL_USART_ClearFlag_IDLE(USART6);
		LL_DMA_DisableStream(DMA2,LL_DMA_STREAM_1);


		LL_DMA_ClearFlag_DME1(DMA2);
		LL_DMA_ClearFlag_HT1(DMA2);
		LL_DMA_ClearFlag_TC1(DMA2);
		LL_DMA_ClearFlag_TE1(DMA2);
		LL_DMA_ClearFlag_FE1(DMA2);

		LL_USART_ClearFlag_CM(USART6);
		LL_USART_ClearFlag_EOB(USART6);
		LL_USART_ClearFlag_FE(USART6);
		LL_USART_ClearFlag_LBD(USART6);
		LL_USART_ClearFlag_NE(USART6);
		LL_USART_ClearFlag_ORE(USART6);
		LL_USART_ClearFlag_PE(USART6);
		LL_USART_ClearFlag_RTO(USART6);
		LL_USART_ClearFlag_TC(USART6);
		LL_USART_ClearFlag_WKUP(USART6);
		LL_USART_ClearFlag_nCTS(USART6);
		LL_USART_ClearFlag_IDLE(USART6);
		uwb.RxDataSize = UWB_RX_LEN - LL_DMA_GetDataLength(DMA2, LL_DMA_STREAM_1);

		do{
			if(uwb.RxRawDat[0] != 0xAU || uwb.RxRawDat[1] != uwb.RxDataSize) break;
			if(uwb.LockRx == HAL_LOCKED)break;
			uwb.LockRx = HAL_LOCKED;
			memcpy(uwb.RxDat,uwb.RxRawDat,uwb.RxDataSize);
			uwb.RxDat[uwb.RxDataSize]=0;
			uwb.LockRx = HAL_UNLOCKED;
			uwb.RxFlag = true;   //收到完整一帧

			BaseType_t YieldRequired = xTaskResumeFromISR(uwbTaskHandle);
			if(YieldRequired==pdTRUE)
			{
				/*如果函数xTaskResumeFromISR()返回值为pdTRUE，那么说明要恢复的这个
				任务的任务优先级等于或者高于正在运行的任务(被中断打断的任务),所以在
				退出中断的时候一定要进行上下文切换！*/
				portYIELD_FROM_ISR(YieldRequired);
			}

		}while(0);

		LL_DMA_SetDataLength(DMA2, LL_DMA_STREAM_1, UWB_RX_LEN);
		LL_DMA_EnableStream(DMA2, LL_DMA_STREAM_1);
	}
}

void UWB::uwb_Init()
{
	LockRx = HAL_UNLOCKED;
	RxFlag = false;
	RxDataSize = 0;
	executionTime_us = 0;
	Sta = STA_INI;
	Err = ERR_NONE;

	/* 配置接收DMA */
	LL_DMA_DisableStream(DMA2,LL_DMA_STREAM_1);
	LL_DMA_SetPeriphAddress(DMA2, LL_DMA_STREAM_1, (uint32_t)&huart->RDR);
	LL_DMA_SetMemoryAddress(DMA2, LL_DMA_STREAM_1, (uint32_t)RxRawDat);
	LL_DMA_SetDataLength(DMA2, LL_DMA_STREAM_1, UWB_RX_LEN);
	LL_DMA_EnableStream(DMA2, LL_DMA_STREAM_1);
	/* 配置接收DMA */

	LL_DMA_ClearFlag_DME1(DMA2);
	LL_DMA_ClearFlag_HT1(DMA2);
	LL_DMA_ClearFlag_TC1(DMA2);
	LL_DMA_ClearFlag_TE1(DMA2);
	LL_DMA_ClearFlag_FE1(DMA2);
////
	LL_USART_ClearFlag_CM(huart);
	LL_USART_ClearFlag_EOB(huart);
	LL_USART_ClearFlag_FE(huart);
	LL_USART_ClearFlag_LBD(huart);
	LL_USART_ClearFlag_NE(huart);
	LL_USART_ClearFlag_ORE(huart);
	LL_USART_ClearFlag_PE(huart);
	LL_USART_ClearFlag_RTO(huart);
	LL_USART_ClearFlag_TC(huart);
	LL_USART_ClearFlag_WKUP(huart);
	LL_USART_ClearFlag_nCTS(huart);
	LL_USART_ClearFlag_IDLE(huart);


	LL_USART_EnableDMAReq_RX(huart);
	LL_USART_ClearFlag_IDLE(huart);
	LL_USART_EnableIT_IDLE(huart);
	Sta = STA_RUN;
	osDelay(100);
}

void UWB::uwb_Update()
{
	startTimerLast = startTimer;
	getTimer_us(&startTimer);
	cycleTime_us = startTimer - startTimerLast;

	if(LockRx == HAL_LOCKED)return;
	LockRx = HAL_LOCKED;
	do{
		uint8_t num = RxDat[1];
		RxSum = 0;
		for(uint8_t i=0; i<num-1; i++) RxSum += RxDat[i];
		if(RxSum != RxDat[num-1])break;		//和校验 判断sum

		for(uint8_t i=0;i<3;i++)
		{
			memcpy(temp.uc,&RxDat[3+i*4],4);
			uwbPos.uwbPos[i] = temp.fp;
		}
		xQueueOverwrite(queueUWB,&uwbPos);

	}while(0);
	LockRx = HAL_UNLOCKED;
	getTimer_us(&stopTimer);

	executionTime_us = stopTimer - startTimer;
}

extern "C" void uwb_main(void *argument)
{
	uwb.uwb_Init();
	osDelay(20);
	for(;;)
	{
		vTaskSuspend(uwbTaskHandle);
		uwb.uwb_Update();
	}
}

/************************ (C) COPYRIGHT Longmen Drone Team *****END OF FILE****/
